package novemberBot.navigation;

import novemberBot.FactoryFloorRobot;
import novemberBot.controller.ProximityFetcher;
import novemberBot.mapmaking.Map;
import novemberBot.mapmaking.PathFinder;
import novemberBot.mapmaking.Vertice;
import novemberBot.utils.Debugger;
import novemberBot.utils.Trig;


/**Navigator is a thread that controls the motion of the robot.
 * Waits for setMotion to be called and executes the required motion.
 * Has options for immediate return.
 * Forward and rotational speeds in cm / s and degrees / s, respectively
 * @author Antoine Lennartz*/
public class Navigator extends Thread{
	
	/**Default values for forward and rotation speeds*/
	private static final double FORWARD_SPEED=6.5, ROTATION_SPEED=30;
	/**error tolerance for final position*/
	private static final int FORWARD_TOLERANCE=3, ROTATION_TOLERANCE=3;
	/**link to main robot*/
	private FactoryFloorRobot ffr;
	/**link to main Odometer*/
	private Odometer odo;
	/**link to speed controller*/
	private SpeedController sc;	
	/**current target position*/
	private double[] target = new double[3];
	/**map data for cinderBlocks*/
	private byte[][] obstacleMap;
	/**map data for pallets*/
	private byte[][] palletMap;
	/**grid snapper class*/
	private GridSnapper gs;
	/**boolean for path motions*/
	private boolean pathMotion=false;
	/**status for navigator*/
	private boolean navigating=false;
	/**first vertice of path*/
	private Vertice start;
	/**link to tom ... otherwise referred to as theOvermind ... but not in the presence of theCaptain, he thinks he's in charge*/
	private PathFinder tom;
	/**theCaptain <b>is</b> in charge of pallet pickup*/
	public ProximityFetcher pf;
	
	private boolean useSnap=false, allowReverse=false;
	
	/**Constructor initializes */
	public Navigator(){
		ffr=FactoryFloorRobot.getInstance();
		odo=ffr.getOdometer();
		pf=new ProximityFetcher();
		ffr.getUsPoller().addResponder(pf);
		gs= new GridSnapper(true);
		sc=new SpeedController();
		this.start();
	}
	
	public void setPathFinder(PathFinder tom){
		this.tom=tom;
	}
	/**Converts the set motion to target coordinate and then tell the captain that it has a motion to perform*/
	public void setRotation(double angle, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		target[0]=cPos[0];
		target[1]=cPos[1];
		target[2]=angle;
		allowReverse=false;
		Thread.yield();
		long wait = System.currentTimeMillis();
		this.interrupt();
		while(!ffr.isMoving() && Math.abs(angle)>ROTATION_TOLERANCE && (wait-System.currentTimeMillis())>1000)Thread.yield();
		if(immediateReturn) return;
		while(ffr.isMoving()) Thread.yield();
	}
	
	/**Converts the set motion to target coordinate and then tell the captain that it has a motion to perform
	 * @param dist - distance to travel
	 * @param immediateReturn - if false does not retur thread till motion is complete*/

	public void setForward(double dist, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		target[0]=cPos[0]+dist*Trig.sin2(cPos[2]);
		target[1]=cPos[1]+dist*Trig.cos2(cPos[2]);
		target[2]=cPos[2];
		allowReverse=true;//allowReverse=true;
		Thread.yield();
		this.interrupt();
		while(!ffr.isMoving() && Math.abs(dist)>FORWARD_TOLERANCE)Thread.yield();
		if(immediateReturn) return;
		while(ffr.isMoving()) Thread.yield();
	}
	/**Converts the set motion to target coordinate and then tell the captain that it has a motion to perform
	 * @param target - target position
	 * @param immediateReturn returns immediately or not from this method*/

	public void setTarget(double[] target, boolean immediateReturn){
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		this.target[0]=target[0];
		this.target[1]=target[1];
		if(target.length==2)
			this.target[2]= Trig.atan3(target[0]-cPos[0], target[1]-cPos[1]);
		else 
			this.target[2]=Trig.fixAngle(target[2]);
		allowReverse=false;
		Thread.yield();
		this.interrupt();
		while(!ffr.isMoving() && ( Trig.distance(target, cPos)>FORWARD_TOLERANCE || Math.abs(Trig.fixAngle2(target[2]-cPos[2]))>ROTATION_TOLERANCE ))Thread.yield();
		if(immediateReturn) return;
		while(ffr.isMoving()) Thread.yield();
	}
	/**Converts the set motion to target coordinate and then tell the captain that it has a motion to perform
	 * @param start - start Vertice
	 * @param immediateReturn returns immediately or not from this method*/
	public void setPath(Vertice start, boolean immediateReturn){
		this.start=start;
		pathMotion=true;
		Thread.yield();
		this.interrupt();
		while(!ffr.isMoving())Thread.yield();
		if(immediateReturn) return;
		while(ffr.isMoving()) Thread.yield();
		
	}
	
	
	/**Sleeps the Navigator until SetMotion is called, then it performs the motion
	 * and goes back to sleep*/
	public void run() {
		while(true){
			try {
				Thread.sleep(60000);
			} catch (InterruptedException e) {
				navigating=true;
				if(pathMotion)executePath();
				else moveToTarget();
				navigating=false;
			}
		}
	}
	/**called by run() executes the current path*/
	private void executePath(){
		Vertice current=start;
		double[] cPos = new double[3];
		double[] dest;
		int state=0;
		if(current.pathNext==current)Debugger.fail();
		while(current.pathNext!=current && state==0){
			dest=Map.odoCoords(current.pathNext.coords);
			target[0]=dest[0];
			target[1]=dest[1];
			odo.getPosition(cPos);
			target[2]=Trig.atan3(target[0]-cPos[0], target[1]-cPos[1]);
			if((state=moveToTarget())!=0)ffr.stop();
			current=current.pathNext;
		}
		if(state!=0){
			//then Path was ended prematurely.
			if(state==2 && ffr.spaceLeft()){
				odo.getPosition(cPos);
				//pallet!!!! YAY
				ffr.roll(-10, 130, false);
				navigating=false;
				pf.driveTo();
				pf.pickUp();
				navigating=true;
				target=cPos;
				
				moveToTarget();
				
			}
			Vertice last=start;

			while(last.pathNext!=last)last=last.pathNext;
			start=tom.makePathTo(Map.odoCoords(last.coords));
			try {
				Thread.sleep(5000);
			} catch (InterruptedException e) {}
			executePath();
		}else{
			Vertice last=start;

			while(last.pathNext!=last)last=last.pathNext;
			if(current.coords.equals(Map.mapCoords(FactoryFloorRobot.DROP_OFF)) && ffr.getLoad()>=1){
				odo.getPosition(cPos);
				target[0]=cPos[0];
				target[1]=cPos[1];
				target[2]=270-45;
				moveToTarget();
				ffr.roll(-10, 150, false);
				
				pf.dropOff();
				
				ffr.roll(15, 150, false);
			}
			//path was completed successfully
			pathMotion=false;
		}
	}
	
	/**performs the motion to the current immediate target*/
	private int moveToTarget(){
		
		int courseState=0;
		double[] scales= new double[2];
		
		ffr.stop();
		sc.setTargPos(target);
		sc.setEnabled(true);
		sc.setAllowReverse(allowReverse);
		
		double dist=sc.getDist(), dAng;
		if(useSnap)gs.setEnabled(true);
		
		//To Point
		while(dist>FORWARD_TOLERANCE){
			sc.getPointScales(scales);
			if(Math.abs(scales[1])==1){
				gs.setEnabled(false);
			}
			else {
				if(useSnap)gs.setEnabled(true);
				courseState=courseClear();
				if(courseState!=0)return courseState;
			}
			ffr.setSpeeds(scales[0]*FORWARD_SPEED, scales[1]*ROTATION_SPEED);
			try {Thread.sleep(Odometer.DEFAULT_PERIOD*30);} catch (InterruptedException e) {}
			dist=sc.getDist();
		}
		ffr.stop();
		
		//To Angle
		gs.setEnabled(false);
		dAng=sc.getdAng();	
		if(Math.abs(dAng)>10)ffr.setSpeeds(0, dAng/Math.abs(dAng)*ROTATION_SPEED);
		while(Math.abs(dAng)>10){
			dAng=sc.getdAng();
			try {Thread.sleep(Odometer.DEFAULT_PERIOD*20);} catch (InterruptedException e) {}
		}
		
		dAng=sc.getdAng();
		while(Math.abs(dAng)>ROTATION_TOLERANCE){
			sc.getAngleScales(scales);
			ffr.setSpeeds(0, scales[1]*ROTATION_SPEED);
			try {Thread.sleep(Odometer.DEFAULT_PERIOD*20);} catch (InterruptedException e) {}
			dAng=sc.getdAng();
		}
		ffr.stop();
		sc.setEnabled(false);
		return 0;
	}	
	/**used to determine weather or not a course is clear*/
	private int courseClear(){
		obstacleMap=ffr.getHighMap().getSensorData();
		palletMap=Map.subtract(ffr.getLowMap(), ffr.getHighMap(), 0.5);
		if(sc.getDist()<15)return 0;//because we are about to stop anyway and we don't want to stop prematurely
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		int[] mapCoords = Map.mapCoords(cPos);
		Odometer.Direction dir =odo.getDirectionCopy();
		if(dir==Odometer.Direction.X_NEG)
			mapCoords[0]--;
		else if(dir==Odometer.Direction.X_POS)
			mapCoords[0]++;
		else if(dir==Odometer.Direction.Y_NEG)
			mapCoords[1]--;
		else if(dir==Odometer.Direction.Y_POS)
			mapCoords[1]++;
	
		
		if(mapCoords[0]<0)mapCoords[0]=0;
		if(mapCoords[0]>12)mapCoords[0]=12;
		if(mapCoords[1]<0)mapCoords[1]=0;
		if(mapCoords[1]>12)mapCoords[1]=12;
		
		if(obstacleMap[mapCoords[0]][mapCoords[1]]>Map.OCCUPIED){
			return 1;
		}
		if(palletMap[mapCoords[0]][mapCoords[1]]>5){
			return 2;
		}
		return 0;
	}
	/**@return true if path underway or pallet pickup in progress(that's not intended)*/
	public boolean isNavigating(){
		return navigating;
	}
	/**enables or disables the use of gridsnapping
	 * an extra boolean was required because the captain needs to turn
	 *  snapping on and off himself while the robot is turning*/
	public void useSnap(boolean useSnap){
		this.useSnap=useSnap;
		gs.setEnabled(useSnap);
	}
	
}
